
#include "AsAttitudeParam_2023.h"
#include "AsCoordinate.h"
#include "AsMath.h"
#include "AsAttitudeParam.h"
#include <iostream>


using namespace std;


///***********************************************************************
/// The conversion from matrix to axis and angle.
/// @Author	Xiao Yao
/// @Date	2023.4.4
/// @Input
/// @Param	mtx	    坐标转换矩阵
/// @Output
/// @Param	axis	旋转轴矢量
/// @Param	angle	旋转角[0, pi]
///***********************************************************************
void AsMtxToAxAng(const CMatrix<double>& mtx,
	CCoord& axis, double& angle)
{

	CMatrix<double> mm = mtx;
	//************** 判断是否为正交矩阵 ******************
	double p1 = mm[0][0] * mm[0][1] + mm[1][0] * mm[1][1] + mm[2][0] * mm[2][1];
	double p2 = mm[0][0] * mm[0][2] + mm[1][0] * mm[1][2] + mm[2][0] * mm[2][2];
	double p3 = mm[0][2] * mm[0][1] + mm[1][2] * mm[1][1] + mm[2][2] * mm[2][1];
	double p4 = mm[0][0] * mm[0][0] + mm[1][0] * mm[1][0] + mm[2][0] * mm[2][0];
	double p5 = mm[0][1] * mm[0][1] + mm[1][1] * mm[1][1] + mm[2][1] * mm[2][1];
	double p6 = mm[0][2] * mm[0][2] + mm[1][2] * mm[1][2] + mm[2][2] * mm[2][2];
	if (!(fabs(p1)<1e-9 && fabs(p2)<1e-9 && fabs(p3)<1e-9 && 
		  fabs(p4-1)<1e-9 && fabs(p5 - 1)<1e-9 && fabs(p6 - 1)<1e-9) ){
		printf("warning! not Orthogonal Matrix\n");
		angle = 0;
		axis[0] = 0;
		axis[1] = 0;
		axis[2] = 0;
		return;
	}

	// 矩阵的迹
	double T = mm[0][0] + mm[1][1] + mm[2][2];

	if (fabs(T - 3) > 1e-9 && fabs(T + 1) > 1e-9) {
		double cc = 0.5 * (T - 1);   // cos(angle)
		double ss = sin(acos(cc));   // sin(angle)
		angle = atan2(ss, cc);       // (0, pi)
		axis[0] = 0.5 * (mm[1][2] - mm[2][1]) / ss;
		axis[1] = 0.5 * (mm[2][0] - mm[0][2]) / ss;
		axis[2] = 0.5 * (mm[0][1] - mm[1][0]) / ss;
	}
	else if (fabs(T + 1) < 1e-9) {
		// 转轴有两个解，指向相差180°,且角度不影响, 本程序仅输出一个
		// sin(angle) = 0， angle=pi+2k*pi,k为整数
		angle = AsCPI;

		double a12 = mm[0][1] / 2.0;
		double a23 = mm[1][2] / 2.0;
		double a31 = mm[2][0] / 2.0;
		double aa1 = sqrt((1.0 + mm[0][0]) / 2.0);
		double aa2 = sqrt((1.0 + mm[1][1]) / 2.0);
		double aa3 = sqrt((1.0 + mm[2][2]) / 2.0);
		if (a12 > 0 && a23 > 0 && a31 > 0) {
			// a1 a2 a3同号
			axis[0] = aa1;
			axis[1] = aa2;
			axis[2] = aa3;
		}
		else if (a12 > 0) {
			// a1 a2 同号
			axis[0] = aa1;
			axis[1] = aa2;
			axis[2] = -aa3;
		}
		else if (a23 > 0) {
			// a2 a3 同号
			axis[0] = aa1;
			axis[1] = -aa2;
			axis[2] = -aa3;
		}
		else {
			// a1 a3同号
			axis[0] = aa1;
			axis[1] = -aa2;
			axis[2] = aa3;
		}
	}
	else {
		// 转轴不确定
		angle = 0;
		axis[0] = 1;
		axis[1] = 0;
		axis[2] = 0;
	}
}


///***********************************************************************
///将四元数转化成欧拉角_123转序
/// @Author	Liu Tianqing
/// @Date	2023.04.06
/// @Input
/// @Param	q		标准化四元数
/// @Output
/// @Param	euler		欧拉角(rad)
///***********************************************************************
bool AsQuatToEuler123(const CQuaternion& q, CEuler& euler)
{
	//设置默认参数
	double epsilon = 1e-6;		//精确度
	euler = { 0, 0, 0 };
	//q0, q1, q2, q3分别对应q.m_Qs, q.m_Qx, q.m_Qy, q.m_Qz
	double q0q0 = q.m_Qs*q.m_Qs;
	double q1q1 = q.m_Qx*q.m_Qx;
	double q2q2 = q.m_Qy*q.m_Qy;
	double q3q3 = q.m_Qz*q.m_Qz;
	double q_norm2 = q0q0 + q1q1 + q2q2 + q3q3;
	if (fabs(q_norm2 - 1) > epsilon)			//判断输入的四元数是否标准化
	{
		cout << "请输入标准化四元数！！！" << endl;
		return false;
	}
	else if (fabs(q0q0 - 1) < epsilon) {}	//没有旋转时，直接输出
	else
	{
		double q0q1 = q.m_Qs*q.m_Qx;
		double q0q2 = q.m_Qs*q.m_Qy;
		double q0q3 = q.m_Qs*q.m_Qz;
		double q1q2 = q.m_Qx*q.m_Qy;
		double q1q3 = q.m_Qx*q.m_Qz;
		double q2q3 = q.m_Qy*q.m_Qz;

		double a02 = q0q0 - q2q2;
		double a13 = q1q1 - q3q3;
		double sy = 2 * (q1q3 + q0q2);   //俯仰角 angle.y 的正弦值

		//判断是否出现奇异，当出现奇异时，令 Angle3或Angle1 为 0，确保程序可以继续运行，这里默认 Angle3为0
		if (sy - 1 >= 0)		//判断俯仰角为pi/2
		{
			euler.m_Angle2 = AsCHalfPI;
			euler.m_Angle1 = 2*atan(q.m_Qx / q.m_Qy);
		}
		else if (sy + 1 <= 0)   //判断俯仰角为-pi/2
		{
			euler.m_Angle2 = -AsCHalfPI;
			euler.m_Angle1 = -2*atan(q.m_Qx / q.m_Qy);
		}
		else
		{
			euler.m_Angle2 = asin(2 * (q1q3 + q0q2));
			euler.m_Angle1 = atan2(2 * (q0q1 - q2q3), a02 - a13);
			euler.m_Angle3 = atan2(2 * (q0q3 - q1q2), a02 + a13);
		}
	}
	return true;
}

/***********************************************************************
将四元数转化成欧拉角_132转序
 @Author	Xiong yanci
 @Date	2025.02.27
 @Input
 @Param	q		标准化四元数
 @Output
 @Param	euler		欧拉角(rad)
***********************************************************************/
bool AsQuatToEuler132(const CQuaternion& q, CEuler& euler)
{
	////设置默认参数
    double epsilon = 1e-6;		//精确度
    euler = { 0, 0, 0 };

	double q0, q1, q2, q3;
	double q0q1, q0q2, q0q3, q1q3, q1q2, q2q3;
	double q0q0, q1q1, q2q2, q3q3;
	double q_norm1,q_norm2;
	double norm_sq;

	q0 = q.m_Qs;
	q1 = q.m_Qx;
	q2 = q.m_Qy;
	q3 = q.m_Qz;

	q0q1 = q0 * q1;
	q0q2 = q0 * q2;
	q0q3 = q0 * q3;
	q1q2 = q1 * q2;
	q1q3 = q1 * q3;
	q2q3 = q2 * q3;
	q0q0 = q0 * q0;
	q1q1 = q1 * q1;
	q2q2 = q2 * q2;
	q3q3 = q3 * q3;

	q_norm1 = q0q0 + q1q1 - q2q2 - q3q3;
	q_norm2 = q0q0 - q1q1 + q2q2 - q3q3;
	norm_sq = q0q0 + q1q1 + q2q2 + q3q3;
	double t = 2 * q0q3 - 2 * q1q2;

	if(fabs(norm_sq - 1) > epsilon)//判断输入的四元数是否标准化
	{
	   cout << "请输入标准化四元数！！！" << endl; 
	   return false;//自动归一化，输出000值
	}
	else
	{   
		//	//判断是否出现奇异，当出现奇异时，令roll = Angle1 为 0，确保程序可以继续运行
		//yaw
		if (t > 1)
		{
          euler.m_Angle3 = AsCHalfPI;
		  euler.m_Angle1 = 2 * atan(q1/q0);
		}
			
		else if (t < -1)
		{
          euler.m_Angle3 = -AsCHalfPI;
		  euler.m_Angle1 = 2 * atan(q1 / q0);
		}
			
		else
		{
          	//XZY四元数转欧拉角
	      euler.m_Angle2 = atan2((2 * q0q2 + 2 * q1q3),q_norm1);  //roll
	      euler.m_Angle1 = atan2((2 * q0q1 + 2 * q2q3),q_norm2);  //pitch
          euler.m_Angle3 = asin(t);               //yaw
		}
		
	}
    return true;
}

///***********************************************************************
///将四元数转化成欧拉角_213转序
/// @Author	Kong Lingyue
/// @Date	2025.03.10
/// @Input
/// @Param	q		标准化四元数
/// @Output
/// @Param	euler		欧拉角(rad)
///***********************************************************************
bool AsQuatToEuler213(const CQuaternion& q, CEuler& euler)
{
	//设置默认参数
	double epsilon = 1e-6;		//精确度
	euler = { 0, 0, 0 };
	//q0, q1, q2, q3分别对应q.m_Qs, q.m_Qx, q.m_Qy, q.m_Qz
	double q0q0 = q.m_Qs*q.m_Qs;
	double q1q1 = q.m_Qx*q.m_Qx;
	double q2q2 = q.m_Qy*q.m_Qy;
	double q3q3 = q.m_Qz*q.m_Qz;
	double q_norm2 = q0q0 + q1q1 + q2q2 + q3q3;
	if (fabs(q_norm2 - 1.0) > epsilon)			//判断输入的四元数是否标准化
	{
		cout << "请输入标准化四元数！！！" << endl;
		return false;
	}
	
	else if (fabs(q0q0 - 1) < epsilon) {}	//没有旋转时，直接输出
	else
	{
		double q0q1 = q.m_Qs*q.m_Qx;
		double q0q2 = q.m_Qs*q.m_Qy;
		double q0q3 = q.m_Qs*q.m_Qz;
		double q1q2 = q.m_Qx*q.m_Qy;
		double q1q3 = q.m_Qx*q.m_Qz;
		double q2q3 = q.m_Qy*q.m_Qz;
		cout << q0q1 << endl;
		double s = -2 * (q2q3 - q0q1);   //正弦值
		//判断是否出现奇异，当出现奇异时，令 Angle3或Angle1 为 0，确保程序可以继续运行，这里默认 Angle3为0
		if (s > 1 - epsilon)		//判断为pi/2
		{
			euler.m_Angle2 = AsCHalfPI;
			euler.m_Angle1 = 2 * atan(q.m_Qy / q.m_Qs);
			euler.m_Angle3 = 0.0;
		}
		else if (s < - 1 + epsilon)   //判断为-pi/2
		{
			euler.m_Angle2 = -AsCHalfPI;
			euler.m_Angle1 = 2 * atan(q.m_Qy / q.m_Qs) ;
			euler.m_Angle3 = 0.0;
		}
		else
		{
			euler.m_Angle2 = asin( s );
			euler.m_Angle1 = -atan2( -2 * (q1q3 + q0q2), 1 - 2 * (q1q1 + q2q2));
			euler.m_Angle3 = -atan2(-2 * (q1q2 + q0q3), 1 - 2 * (q1q1 + q3q3));
		}
	}
	return true;
}


///***********************************************************************
/// 方向余弦阵转321转序欧拉角
/// @Author	Li Sibei
/// @Date	2023.04.06
/// @Input
/// @Param	mtx		方向余弦阵
/// @Output
/// @Param	euler	欧拉角
///***********************************************************************
void AsMtxToEuler321(const CMatrix<double>& mtx, CEuler& euler)
{
	//异常数据处理1：输入非正交矩阵
	CMatrix<double> MTX1(mtx);
	CMatrix<double> mtx1 = MTX1.Transpose();
	CMatrix<double> mtx2 = MTX1.Inv();
	CMatrix<double> mtx3 = mtx1 - mtx2;
	for (int i = 0; i <= 2; i++)
	{
		for (int j = 0; j <= 2; j++)
		{
			if (mtx3[i][j] > 1e-15 || mtx3[i][j] < -1e-15)
			{
				printf("Error: 方向余弦阵非正交矩阵!\n");
				return;
			}
		}
	}
	//异常数据处理2：奇异
	if (mtx[0][2] == 1)
	{
		euler.m_Angle1 = -atan2(mtx[2][1], mtx[1][1]);
		euler.m_Angle2 = -asin(mtx[0][2]);
		euler.m_Angle3 = 0;
		return;
	}
	else if (mtx[0][2] == -1)
	{
		euler.m_Angle1 = atan2(mtx[2][1], mtx[1][1]);
		euler.m_Angle2 = -asin(mtx[0][2]);
		euler.m_Angle3 = 0;
		return;
	}
	//一般情况求解
	else
	{
		euler.m_Angle1 = atan2(mtx[0][1], mtx[0][0]);
		euler.m_Angle2 = -asin(mtx[0][2]);
		euler.m_Angle3 = atan2(mtx[1][2], mtx[2][2]);
	}
}


///***********************************************************************
/// 方向余弦矩阵转为313转序Euler角
/// @Author	Wang Weili
/// @Date	2023.4
/// @Input
/// @Param	mtx		坐标转移矩阵
/// @Output
/// @Param	euler	欧拉角(rad)
///***********************************************************************
void AsMtxToEuler313(const CMatrix<double>& mtx, CEuler& euler)
{
	if (mtx[2][2] < +1)
	{
		if (mtx[2][2] > -1)
		{
			euler.m_Angle1 = atan2(mtx[2][0], -mtx[2][1]);
			euler.m_Angle2 = acos(mtx[2][2]);
			euler.m_Angle3 = atan2(mtx[0][2], mtx[1][2]);
		}
		else //mtx[2][2]==-1
		{
			euler.m_Angle1 = atan2(mtx[0][1], mtx[0][0]);
			euler.m_Angle2 = AsCPI;
			euler.m_Angle3 = 0;
		}
	}
	else //mtx[2][2]==+1
	{
		euler.m_Angle1 = atan2(mtx[0][1], mtx[0][0]);
		euler.m_Angle2 = 0;
		euler.m_Angle3 = 0;
	}
}


///***********************************************************************
/// 方向余弦矩阵转为312转序Euler角
/// @Author	Zhang Xuaying
/// @Date	2023.04.01
/// @Input  
/// @Param	mtx  		方向余弦矩阵
/// @Output	
/// @Param	euler		欧拉角(rad)
///***********************************************************************
void AsMtxToEuler312(const CMatrix<double>& mtx, CEuler& euler)
{
	bool singular = mtx(1, 2) > 1 - 1e-8;//判断是否出现欧拉角奇异问题	
	if (!singular)
	{
		euler.m_Angle2 = asin(mtx(1, 2));
		//偏航
		if (mtx(1, 0) == 0)
		{
			euler.m_Angle1 = 0;
		}
		else
		{
			euler.m_Angle1 = -atan2(mtx(1, 0), mtx(1, 1));
		}

		//俯仰
		if (mtx(0, 2) == 0)
		{
			euler.m_Angle3 = 0;
		}
		else
		{
			euler.m_Angle3 = -atan2(mtx(0, 2), mtx(2, 2));
		}
	}
	else
	{
		cout << "出现欧拉角数值奇异问题 " << endl;
		euler.m_Angle1 = 0;
		euler.m_Angle2 = AsCHalfPI;
		euler.m_Angle3 = atan2(mtx(2, 0), mtx(0, 0));
	}
}


///******************************************************
/// 根据转序和坐标转移矩阵计算Euler角，转序为2-3-1
/// @author Sun Fengyan
/// @data   2023.4
/// @Version    1.0
/// @Input  
/// @Param  mtx       坐标转移矩阵
/// @Output
/// @Param  euler     返回的欧拉角(单位：rad）
///******************************************************
void  AsMtxToEuler231(const CMatrix<double>& mtx, CEuler& euler)
{
	euler.m_Angle2 = asin(mtx[0][1]); 

	if (fabs(mtx[0][1] - 1) < 1e-8 ) //euler.m_Angle2 = pi/2
	{
		euler.m_Angle1 = atan2(mtx[1][2], mtx[2][2]);
		euler.m_Angle3 = 0;
	}
	else if (fabs(mtx[0][1] + 1) < 1e-8 ) //euler.m_Angle2 = -pi/2
	{
		euler.m_Angle1 = -atan2(mtx[1][2], mtx[2][2]);
		euler.m_Angle3 = 0;
	}
	else 
	{
		euler.m_Angle1 = atan2(-mtx[0][2], mtx[0][0]);
		euler.m_Angle3 = atan2(-mtx[2][1], mtx[1][1]);
		
	}
}


///***********************************************************************
/// The conversion from matrix to angle123.
/// @Author	gong he
/// @Date	2023.4.4
/// @Input
/// @Param	mtx	    坐标转换矩阵
/// @Output
/// @Param	eular	旋转角[0, pi]
///***********************************************************************
void AsMtxToEuler123(const CMatrix<double>& mtx,
	CEuler& euler)
{
	CMatrix<double> mm = mtx;
	double theta = 0, psi = 0, pfi = 0;
	if (fabs(mm[2][0]) != 1) {
		if (cos(-asin(mm(2, 0)) >= 0)) {
			theta = -asin(mm(2, 0));
			psi = atan2(mm(2, 1) / cos(theta), mm(2, 2) / cos(theta));
			pfi = atan2(mm(1, 0) / cos(theta), mm(0, 0) / cos(theta));
		}
		else {
			theta = AsCPI - theta;
			psi = atan2(mm(2, 0) / cos(theta), mm(2, 2) / cos(theta));
			pfi = atan2(mm(1, 0) / cos(theta), mm(0, 0) / cos(theta));
		}
	}
	else {
		double pfi = 0;
		double delta = atan2(mm(0, 1), mm(0, 2));
		if (mm[2][0] == -1) {
			theta = AsCPI / 2;
			psi = pfi + delta;
		}
		else {
			theta = -AsCPI / 2;
			psi = -pfi + delta;
		}
	}
	euler.m_Angle1 = pfi;
	euler.m_Angle2 = theta;
	euler.m_Angle3 = psi;
}



///***********************************************************************
/// 将四元数转换为321转序的欧拉角(Z-Y-X)  
/// 遵循航天器标准定义：偏航角->俯仰角->滚转角  
/// 对应绕惯性系Z轴->新Y轴->新X轴的连续旋转  
/// @Author	王薪程（你们可爱的学长哦~） 
/// @Date	2025-03-03  
/// @Version	3.2.1  
///  
/// @Input  
/// @Param	quat	输入四元数(需标准化，不标准化也无所谓啦），顺序:标量Qs,矢量QxQyQz)  
///  
/// @Output  
/// @Param	euler	输出欧拉角结构体(单位：弧度)  
///			        m_Angle3: 绕X轴滚转角 φ ∈ [-π,π] 
///			        m_Angle2: 绕Y轴俯仰角 θ ∈ [-π/2,π/2]  
///			        m_Angle1: 绕Z轴偏航角 ψ ∈ [-π,π]   
///  
/// @Return	true: 转换成功  false: 四元数未标准化  
///  
/// @算法说明：  
/// 1. 通过四元数导出旋转矩阵关键元素，避免完整矩阵计算  
/// 2. 采用显式代数展开提升计算效率  
/// 3. 奇异位置处理逻辑与AsMtxToEuler321()保持兼容  
/// 4. 数值阈值统一采用工程标准1e-7
///***********************************************************************  
void AsQuatToEuler321(CQuaternion& quat, CEuler& euler)
{
	const double eps = 1e-7;          // 工程计算浮点误差阈值  
	euler = { 0.0, 0.0, 0.0 };         // 显式初始化输出参数  

	//============ 四元数标准化验证 ============  
	const double norm_sq = quat.m_Qs * quat.m_Qs +
		quat.m_Qx * quat.m_Qx +
		quat.m_Qy * quat.m_Qy +
		quat.m_Qz * quat.m_Qz;
	if (fabs(norm_sq - 1.0) > eps)
	{
		printf("[ERR]AsQuatToEuler321: 四元数未标准化! 模长平方=%.4f\n", norm_sq);
		quat.Norm();  // 强制标准化四元数
	}

	//======= 计算旋转矩阵关键元素(代数展开式) =======  
	const double q0 = quat.m_Qs;      // 标量部分  
	const double q1 = quat.m_Qx;
	const double q2 = quat.m_Qy;
	const double q3 = quat.m_Qz;

	/* 旋转矩阵R(321转序)关键元素
	[  cy*cz           cy*sz          -sy     ]
	[ sy*sx*cz-sz*cx  sy*sx*sz+cz*cx   cy*sx ]
	[ sy*cx*cz+sz*sx  sy*cx*sz-cz*sx   cy*cx ]
	*/
	// 仅计算必要元素以提升性能  
	double R02 = 2 * (q1 * q3 - q0 * q2);     // -sy*cy ≈ -sin(theta)  
	double R00 = 1 - 2 * (q2 * q2 + q3 * q3); // cy*cz  
	double R01 = 2 * (q1 * q2 + q0 * q3);     // cy*sz  
	double R12 = 2 * (q2 * q3 + q0 * q1);     // cy*sx  
	double R22 = 1 - 2 * (q1 * q1 + q2 * q2); // cy*cx  

	//========= 俯仰角计算(保证θ∈[-π/2,π/2]) ========= 
	if (R02 > 1) R02 = 1;  // 防止数值误差导致acos溢出
	if (R02 < -1) R02 = -1;  // 防止数值误差导致acos溢出
	euler.m_Angle2 = -asin(R02);  // 对应321转序第二个旋转  

	//======= 万向节锁处理(θ≈±π/2时) =======  
	if (fabs(R02) > 1.0 - eps)
	{
		/* 奇异情况处理逻辑
		   当θ=±π/2时，滚转角φ与偏航角ψ不可唯一确定
		   约定固定ψ=0，通过矩阵元素计算φ
		   与方向余弦阵转换函数AsMtxToEuler321()保持兼容
		*/
		euler.m_Angle1 = 0;  // 固定偏航角  

		// 根据R22符号确定θ的正负  
		const double sign_theta = (R02 > 0) ? -1.0 : 1.0;

		/* 计算滚转角φ
		   使用R12和R22元素：
		   sy*cx = R12/(2*cy)
		   cy*cx = R22
		   当θ→±π/2时，cy→0，改用q0和q1计算
		*/
		euler.m_Angle3 = 2 * atan2(q1, q0);
	}
	else
	{
		//======= 常规情况计算 ========  
		// 偏航角ψ(z轴): atan2(cy*sz, cy*cz)  
		euler.m_Angle1 = atan2(R01, R00);

		// 滚转角φ(x轴): atan2(cy*sx, cy*cx)  
		euler.m_Angle3 = atan2(R12, R22);
	}

	//===== 角度范围修正 =====  
	// 保证ψ∈[-π,π]，φ∈[-π,π]  
	if (euler.m_Angle3 > AsCPI) euler.m_Angle3 -= 2 * AsCPI;
	if (euler.m_Angle3 < -AsCPI) euler.m_Angle3 += 2 * AsCPI;
	if (euler.m_Angle1 > AsCPI) euler.m_Angle1 -= 2 * AsCPI;
	if (euler.m_Angle1 < -AsCPI) euler.m_Angle1 += 2 * AsCPI;

	//// 测试用，转为角度制
	//euler.m_Angle3 = euler.m_Angle3 * 180 / AsCPI;
	//euler.m_Angle2 = euler.m_Angle2 * 180 / AsCPI;
	//euler.m_Angle1 = euler.m_Angle1 * 180 / AsCPI;
}

///***********************************************************************
/// 四元数323转换成Euler角
/// @Author	BXT
/// @Date	2025.3
/// @Input
/// @Param quat		四元数
/// @Output
/// @Param euler	欧拉角
///***********************************************************************
void AsQuatToEuler323(CQuaternion& quat, CEuler& euler)
{
	const double eps = 1e-8;

	// 四元数标准化验证
	const double norm_sq = quat.m_Qs * quat.m_Qs + quat.m_Qx * quat.m_Qx + quat.m_Qy * quat.m_Qy + quat.m_Qz * quat.m_Qz;
	if (fabs(norm_sq - 1.0) > eps)
	{
		printf("[ERR]AsQuatToEuler321: 四元数未标准化! 模长平方=%.4f\n", norm_sq);
		return;
	}

	// 将四元数转换为方向余弦矩阵
	CMatrix<double> m(3, 3);
	AsQuatToMtx(quat, m);

	// 提取方向余弦矩阵中的元素
	double m11 = m[0][0];
	double m12 = m[0][1];
	double m13 = m[0][2];
	double m21 = m[1][0];
	double m22 = m[1][1];
	double m23 = m[1][2];
	double m31 = m[2][0];
	double m32 = m[2][1];
	double m33 = m[2][2];

	// 计算ZYZ顺序的欧拉角
	// 参考公式：
	// 旋转矩阵m(323转序)
	// m = [cos(a)*cos(b)*cos(c)-sin(a)*sin(c), cos(a)*sin(c)+cos(b)*cos(c)*sin(a), -cos(c)*sin(b)
	//	   -cos(c)*sin(a)-cos(a)*cos(b)*sin(c), cos(a)*cos(c)-cos(b)*sin(a)*sin(c), sin(b)*sin(c)
	//	   cos(a)*sin(b), sin(a)*sin(b), cos(b)]
	// θ = acos(m33)
	// φ = atan2(m32, m31)
	// ψ = atan2(m23, -m13)

	// 计算theta并判断奇异情况,θ∈[0,π]
	double cosTheta = m33;
	euler.m_Angle2 = acos(cosTheta);

	double sinTheta = sqrt(1.0 - cosTheta*cosTheta);
	if (sinTheta > eps) 
	{
		// 非奇异情况，正常计算
		euler.m_Angle1 = atan2(m32, m31);
		euler.m_Angle3 = atan2(m23, -m13);
	}
	else 
	{
		// 奇异情况：theta接近0或π
		// theta接近0
		if (cosTheta > 0) 
		{
			euler.m_Angle1 = atan2(m12, m11);
			euler.m_Angle3 = 0.0;
		}
		// theta接近π
		else 
		{
			euler.m_Angle1 = atan2(-m21, -m11);
			euler.m_Angle3 = 0.0;
		}
	}

	// 确保m_Angle1和m_Angle3在[-π,π]
	if (euler.m_Angle3 > AsCPI) euler.m_Angle3 -= 2 * AsCPI;
	if (euler.m_Angle3 < -AsCPI) euler.m_Angle3 += 2 * AsCPI;
	if (euler.m_Angle1 > AsCPI) euler.m_Angle1 -= 2 * AsCPI;
	if (euler.m_Angle1 < -AsCPI) euler.m_Angle1 += 2 * AsCPI;

}

///***********************************************************************
/// 根据坐标转移矩阵计算132转序欧拉角
/// @Author	Wang Deyu
/// @Date	2025.03.15
/// @Input
/// @Param	mtx     输入的转移矩阵
/// @Output	
/// @Param	euler	欧拉角    rad
///***********************************************************************
void AsMtxToEuler132(const CMatrix<double>& mtx, CEuler& euler)
{
	double cbeta;
    //处理万向锁	
	if (fabs(-mtx[1][0] - 1) < 1e-12)
	{
		euler.m_Angle1 = 0;
		euler.m_Angle2 = AsCPI / 2;
		euler.m_Angle3 = atan2(-mtx[0][2], mtx[0][1]);
	}
	else if(fabs(-mtx[1][0] + 1) < 1e-12)
	{
		euler.m_Angle1 = 0;
		euler.m_Angle2 = -AsCPI / 2;
		euler.m_Angle3 = atan2(-mtx[0][2], -mtx[0][1]);
	}
	else
	{
		cbeta = sqrt(mtx[1][1] * mtx[1][1] + mtx[1][2] * mtx[1][2]);
		euler.m_Angle1 = atan2(mtx[1][2] / cbeta, mtx[1][1] / cbeta);
		euler.m_Angle2 = asin(-mtx[1][0]);
		euler.m_Angle3 = atan2(mtx[2][0] / cbeta, mtx[0][0] / cbeta);
	}
}

///***********************************************************************
/// 方向余弦矩阵转为212转序Euler角
/// @Author	Chang Xiaokuan
/// @Date	2025.2.28
/// @Input
/// @Param	mtx		坐标转移矩阵
/// @Output
/// @Param	euler	欧拉角(rad)
///***********************************************************************
void AsMtxToEuler212(const CMatrix<double>& mtx, CEuler& euler)
{	
	double epsilon = 1e-12;
	if (mtx[1][1]<(1 + epsilon) & mtx[1][1]>(-1 - epsilon))
	{
		euler.m_Angle2 = acos(mtx[1][1]);
	}
	else
	{
		cout << "error，输入方向余弦矩阵存在大于1或小于-1的元素";
	}

	if (fabs(sin(euler.m_Angle2)) < epsilon)
	{
		euler.m_Angle1 = 0;
		euler.m_Angle3 = atan2(mtx[0][2], mtx[0][0]);
	}
	else //sin(theta) != 0
	{
		euler.m_Angle1 = AsCPI-atan2(mtx[1][0], -mtx[1][2]);
		euler.m_Angle3 = AsCPI-atan2(mtx[0][1], mtx[2][1]);
	}
}

///***********************************************************************
/// 方向余弦矩阵转为121转序Euler角
/// @Author  Ma Zihui
/// @Date	20235.3
/// @Input
/// @Param	mtx		坐标转移矩阵
/// @Output
/// @Param	euler	欧拉角(rad)
///***********************************************************************
void AsMtxToEuler121(const CMatrix<double> &mtx,
	CEuler &euler)
{
	if (fabs(mtx[0][0]) != 1)
	{
		euler.m_Angle1 = atan2(mtx[0][1], -mtx[0][2]);
		euler.m_Angle2 = acos(mtx[0][0]);
		euler.m_Angle3 = atan2(mtx[1][0], mtx[2][0]);
	}
	else
	{
		euler.m_Angle1 = 0;
		double summ = atan2(-mtx[2][1], mtx[1][1]);
		if (mtx[0][0] == -1)
		{
			euler.m_Angle2 = AsCPI;
			euler.m_Angle3 = summ;
		}
		else
		{
			euler.m_Angle2 = 0;
			euler.m_Angle3 = summ;
		}
	}
}